import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3a class. The robots runs toward a light bulb and runs into it.
 * Bang-bang controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller4a extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		int left = 0;
		int right = 0;
		int check = 400;

		// Vorne - halb links
		if (getLightValue(1) < check) {
			left += 1;
		}
		// Vorne links
		if (getLightValue(2) < check) {
			left += 2;
		}
		// Vorne rechts
		if (getLightValue(3) < check) {
			right += 2;
		}
		// Vorne halb rechts
		if (getLightValue(4) < check) {
			right += 1;
		}

		if (left < right) {
			setMotorSpeeds(5, 4);
		} else {
			if (right < left) {
				setMotorSpeeds(4, 5);
			} else {
				setMotorSpeeds(5, 5);
			}
		}

	}
}
